
#include <stdio.h>
#include <stdint.h>
#include <iostream>

#include "basic.h"
#include "timeSync.h"
#include "LooseCoupling.h"
#include "./matlab/loadtxt.h"

#pragma pack(push, 8)
typedef struct
{
    struct
    {
        Vec3 gyr, acc;
        double tk;
    } imu;
    struct
    {
        Vec3 vn, pos;
        double tk;
    } gnss;
} sensorData_s;

typedef struct
{
    Vec15 xk;
    Vec3 eb, db;
    Vec15 Pkk; // diag(Pk)
    double tk;
} kf_output_s;
#pragma pack(pop)

CLCKF inavApp;

CVec3 att0(-0.00883976661208674, 0.00861164726773167, -0.0261422081005676);
CVec3 vn0 (0.15, 0.15, 0.15);
CVec3 pos0(0.506145639863950, 1.85004916389958, 453);

#define NUM 16
double avpOut[NUM] = {0.0};

int main()
{
    printf("CINS occupying %f kb \r\n", sizeof(inavApp) / 1024.0);
    // printf("'CLCKF' occupying %f kb memory\r\n", sizeof(LCApp) / 1024.0f);
    loadInTxt("./matlab/imugps.txt");
    creatOutTxt("./matlab/output.txt");
    sensorData_s *dd = (sensorData_s *)&dataIn;
    kf_output_s kfOut = {0};

    inavApp.init(att0, vn0, pos0, dd->imu.tk);

    while (readLine())
    {
        inavApp.sins_update(dd->imu.gyr, dd->imu.acc, dd->imu.tk, 1);
        inavApp.time_update(dd->imu.tk);

        if(dd->gnss.tk > 0)
        {
            inavApp.gnssMeas_update(dd->gnss.tk, Eigen::Map<CVec3>(dd->gnss.vn), Eigen::Map<CVec3>(dd->gnss.pos), true);
            inavApp.feedback(0xffff);
        }

        memcpy((void *)&avpOut[0],  (void *)inavApp.sins.att.data(), 3 * sizeof(double));
        memcpy((void *)&avpOut[3],  (void *)inavApp.sins.vn.data(),  3 * sizeof(double));
        memcpy((void *)&avpOut[6],  (void *)inavApp.sins.pos.data(), 3 * sizeof(double));
        memcpy((void *)&avpOut[9],  (void *)inavApp.sins.eb.data(),  3 * sizeof(double));
        memcpy((void *)&avpOut[12], (void *)inavApp.sins.db.data(),  3 * sizeof(double));
        avpOut[NUM-1] = inavApp.tk;
        outLine(avpOut, NUM);
    }
    
    closeOutTxt();
    closeInTxt();

    // LCApp.sins_update();
    // LCApp.time_update();

    /* GNSS-PVT、Zero-Velocity、Zero-Angular-Rate detection */

    // switch (NAV_PROCESS)
    // {
    //     case Init_Alignment:
    //         /* code */
    //         break;

    //     case INav_GNSSFuse:
    //         break;

    //     case Dead_Reckoning:
    //         break;
            
    //     case ZARUPT:
    //         break;

    //     default: 
    //         /* ZUPT */
    //         break;
    // }           
}
